import json
import math
import os
import time
from typing import List, Tuple, Optional
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # 使用黑体
plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题
import numpy as np

import scenario
from agent import Drone, Boat, Agent, AGENT_PARAMS, TaskType
from multi_target_prob_map import MultiTargetProbabilityMap
# from hybrid_astar_framework.hybrid_a_n_with_orca_and_time_windows import ORCAPlanner, PathTrackerPID, TerminalControllerPID, TimeAwareController, VehicleDynamics
from explore import plan_explore_area


# TODO:按照场景想定的单位位置初始化
# TODO:封锁区的实现
class World:
    """
    世界类，管理仿真环境和所有智能体
    """

    def __init__(self, width: float, height: float, interval: float, visualize: bool = True,
                 grid_resolution: int = 1000, debug_mode: bool = False, case: int = 1):
        self.width = width  # 世界宽度
        self.height = height  # 世界高度
        self.interval = interval  # 仿真步长
        self.all_agents = []  # 所有智能体列表，包括已被击毁退出的
        self.agents = []  # 所有存活智能体列表
        self.faction_agents = {}  # 按阵营区分的智能体列表
        self.faction_detected = {}  # 按阵营区分的态势感知情况，脱离视野的单位会被移除
        self.faction_bsv_detected = {}  # 按阵营区分的敌方BSV探测情况，脱离视野也保持记录
        self.faction_maps = {}  # 按阵营存储态势地图
        self.faction_attacking = {}  # 按阵营区分的正在被锁定的目标
        self.faction_attacking_cache = {}  # 缓存正在被锁定的目标
        self.grid_resolution = grid_resolution
        self.time = 0.0  # 仿真时间
        self.step = 0  # 仿真步数
        self.max_step = 10000000
        self.break_through = []  # 成功突防单位列表
        self.break_through_time = 3600 * 100
        self.white_collision_count = 0
        self.white_freeze_count = 0
        self.black_freeze_count = 0
        self.running = False  # 仿真运行标志
        self.winner = None  # 最终胜利的阵营

        # 日志相关属性
        self.log_interval = 10  # 每10步记录一次日志
        self.log_data = {}  # 存储日志数据的字典
        # 新增态势感知评估指标
        self.prediction_errors = []  # 存储每个时间步的平均预测误差
        # 新增探测时间统计
        self.detection_time_ratios = {}  # {target_id: (总存在时间, 被探测时间)}
        self.avg_detection_ratio = 0.0   # 平均探测时间占比
        # 新增熵值统计
        self.entropy_history = []          # 每步的平均熵值
        self.low_entropy_count = 0.0 # 低熵状态步数
        self.avg_entropy = 0.0 # 平均熵值
        self.low_entropy_ratio = 0.0  # 低熵状态占比

        # 决策
        self.global_coordinator = None
        # USV进入初始防御阵位
        self.boat_occupy_position = True
        self.defense_line_x = 50000  # 初始防御阵位横坐标
        self.num_zones = 3  # 防御阵位区域数量

        # 调试模式
        self.debug_mode = debug_mode
        self.cbba_call_count = 0  # CBBA调用次数
        self.last_cbba_call_step = 0  # 上次CBBA调用的步数

        # 控制参数
        self.speed_factor = 100  # 加速倍率
        self.paused = False  # 暂停状态
        self.show_ranges = True  # 是否显示范围
        self.show_text = True  # 是否显示文本标签

        # 可视化相关
        self.visualize = visualize
        self.visualizer = None
        if visualize:
            from visualizer import Visualizer
            self.last_draw_step = 0
            self.visualizer = Visualizer(self)
            self.visualizer.on_key_press = self.on_key_press  # 设置键盘事件回调
            self.visualizer.init_visualization()

        # 从配置文件加载预设方案
        self.case = case
        self.load_black_agents_from_scheme(scenario.get_scenario_path(self.case))

    def add_agent(self, agent: Agent):
        """添加智能体到世界中"""
        self.agents.append(agent)
        self.all_agents.append(agent)
        # 将智能体加入到按阵营区分的智能体列表中
        if agent.faction not in self.faction_agents:
            self.faction_agents[agent.faction] = []
        if agent.faction not in self.faction_detected:
            self.faction_detected[agent.faction] = set()
            self.faction_bsv_detected[agent.faction] = set()
            self.faction_attacking[agent.faction] = set()
            self.faction_attacking_cache[agent.faction] = set()
        self.faction_agents[agent.faction].append(agent)
        agent.world = self

        # 添加可视化对象
        if self.visualize and self.visualizer:
            self.visualizer.create_visual_object(agent)

        # 初始化黑方目标的探测时间统计
        if agent.faction == 'black':
            self.detection_time_ratios[agent.id] = [0, 0]  # [总存在时间, 被探测时间]

        # 初始化控制器
        # terminal_gains = {
        #     'Kp_v': 1.5, 'Kp_w': 4.5, 'Ki_w': 1.0, 'Kd_w': 2.0,
        #     'align_distance': 60.0, 'final_align_gain': 12.0
        # }
        # agent.tracking_controller = PathTrackerPID([], agent.max_speed, self.interval, Kp_v=0.8, Kp_w=1.5, Ki_w=0.1, Kd_w=3.5,
        #                                    target_lookahead=3.0)
        # agent.time_controller = TimeAwareController([], [], agent.max_speed)
        # agent.terminal_controller = TerminalControllerPID([], self.interval, **terminal_gains)
        # agent.ego_vehicle = VehicleDynamics([agent.x, agent.y, agent.yaw, math.sqrt(agent.vx**2 + agent.vy**2), agent.omega], self.interval, agent.max_speed, agent.max_accel,
        #                                     agent.max_rot_speed, agent.max_rot_accel)
        # agent.local_planner = ORCAPlanner()

    def remove_agent(self, agent):
        """从世界中移除智能体"""
        if agent in self.agents:
            # 从按阵营区分的智能体列表中移除智能体
            if agent.faction in self.faction_agents and agent in self.faction_agents[agent.faction]:
                self.faction_agents[agent.faction].remove(agent)
            # 从世界智能体列表中移除智能体
            self.agents.remove(agent)
            agent.world = None

            # 移除可视化对象
            if self.visualize and self.visualizer:
                self.visualizer.remove_visual_object(agent.id)

    def update(self):
        """更新世界状态"""
        if self.paused:
            return

        step_start = time.time()

        # 更新各阵营态势图和探测目标列表
        maps_start = time.time()
        per_faction_map_times = []
        for faction in self.faction_agents:
            t0 = time.time()
            if faction == 'white': self.update_situation_map(faction)
            per_faction_map_times.append((faction, time.time() - t0))
        maps_elapsed = time.time() - maps_start

        # 阵营级高层决策
        hld_start = time.time()
        per_faction_hld_times = []
        for faction in self.faction_agents:
            t0 = time.time()
            self.high_level_decision(faction)
            per_faction_hld_times.append((faction, time.time() - t0))
        hld_elapsed = time.time() - hld_start

        # 更新所有智能体
        agents_start = time.time()
        for agent in self.agents[:]:  # 使用副本遍历，允许在更新中移除智能体
            agent.update()
        agents_elapsed = time.time() - agents_start

        for faction in self.faction_agents:
            self.faction_attacking_cache[faction] = self.faction_attacking[faction].copy()
            self.faction_attacking[faction].clear()  # 清空上一步的正在攻击目标列表

        # 检测碰撞
        coll_start = time.time()
        self.check_collisions()
        coll_elapsed = time.time() - coll_start

        # 记录日志（每log_interval步记录一次）
        log_elapsed = 0.0
        if self.step % self.log_interval == 0:
            log_start = time.time()
            self.record_log()
            log_elapsed = time.time() - log_start

        # 检测胜利者
        win_start = time.time()
        self.check_winning()
        win_elapsed = time.time() - win_start

        total_elapsed = time.time() - step_start

        # 打印各部分耗时（仅调试模式下）
        # if self.debug_mode:
        #     map_detail = ", ".join([f"{f}:{t:.4f}s" for f, t in per_faction_map_times])
        #     hld_detail = ", ".join([f"{f}:{t:.4f}s" for f, t in per_faction_hld_times])
        #     print(
        #         f"[Step {self.step}] times - maps:{maps_elapsed:.4f}s ({map_detail}); "
        #         f"highlvl:{hld_elapsed:.4f}s ({hld_detail}); agents:{agents_elapsed:.4f}s; "
        #         f"collisions:{coll_elapsed:.4f}s; log:{log_elapsed:.4f}s; win:{win_elapsed:.4f}s; "
        #         f"total:{total_elapsed:.4f}s"
        #     )

        self.time += self.interval
        self.step += 1

    def record_log(self):
        """记录当前步所有智能体状态到日志"""
        step_log = {"white": {}, "black": {}}

        # 处理白方智能体
        for agent in self.all_agents:
            if isinstance(agent, Boat):
                if agent.world is None:
                    state = "退出"
                elif agent.freeze:
                    state = "冻结"
                elif agent._target is not None:
                    state = "锁定"
                else:
                    state = "移动"
                agent_info = {
                    "pos": [agent.x, agent.y, 0],  # 位置 (x, y, z=0)
                    "type": "无人艇",  # 类型（无人艇）
                    "group": "白" if agent.faction == 'white' else "黑",  # 阵营
                    "state": state,  # 状态（移动/锁定/冻结/退出）
                    "velocity": [agent.vx, agent.vy, 0]  # 速度 (vx, vy, vz=0)
                }
            else:
                if agent.world is None and self.step != 0:
                    state = "退出"
                elif agent.freeze:
                    state = "补能"
                elif agent.need_docking:
                    state = "停泊"
                else:
                    state = "移动"
                agent_info = {
                    "pos": [agent.x, agent.y, 1000 if agent.docking is None else 0],  # 3D位置 (x, y, z)
                    "type": "无人机",  # 类型（无人机）
                    "group": "白" if agent.faction == 'white' else "黑",  # 阵营
                    "state": state,  # 状态（移动/锁定/冻结/退出）
                    "velocity": [agent.vx, agent.vy, 0]  # 3D速度 (vx, vy, vz)
                }
            step_log[agent.faction][agent.id] = agent_info

        # 将当前步日志添加到日志数据中
        self.log_data[str(self.step)] = step_log

    def init_situation_maps(self, grid_resolution=500):
        """为每个阵营初始化态势地图"""
        for faction in self.faction_agents:
            map = MultiTargetProbabilityMap(self.width, self.height, grid_resolution)
            for f in self.faction_agents:
                if f is faction: continue  # 跳过本阵营
                for agent in self.faction_agents[f]:
                    if isinstance(agent, Boat):
                        if agent.faction == 'white':
                            map.add_target(agent.id, [agent.x, agent.y])
                        else:
                            init_prob = np.zeros((map.grid_width, map.grid_height))
                            for x in range(map.grid_width):
                                for y in range(map.grid_height):
                                    if x * grid_resolution > self.width - 50000:
                                        init_prob[x, y] = 1
                            init_prob = init_prob / init_prob.sum()
                            map.add_target_with_prob(agent.id, init_prob)
            self.faction_maps[faction] = map

    def init_high_level_decision(self):
        from global_coordinator import GlobalCoordinator
        for agent in self.faction_agents['white']:
            agent._create_blackboard()
        self.global_coordinator = GlobalCoordinator(self.max_step, self.width, self.height, world=self)
        # 为white阵营启用HTN全局协调
        try:
            self.global_coordinator.start_coordination()

        except Exception as e:
            print(f"HTN全局协调失败: {e}")
            pass  # 如果HTN框架不可用，继续使用默认逻辑

    def update_situation_map(self, faction):
        """更新阵营的态势地图"""
        if faction not in self.faction_maps:
            return

        # 0. 清理阵营发现目标列表
        self.faction_detected[faction].clear()

        # 1. 预测步
        if self.step % 120 == 0: self.faction_maps[faction].predict()

        # 2. 获取所有活跃探测器
        detectors = [agent for agent in self.faction_agents.get(faction, [])
                     if (hasattr(agent, 'sensor_range') and not agent.freeze)]

        # 3. 使用每个探测器的观测更新地图和目标列表
        for detector in detectors:
            detected_targets = detector.sensor_search()
            self.faction_maps[faction].update(detector, detected_targets)
            for target in detected_targets:
                self.faction_detected[faction].add(target)
                self.faction_bsv_detected[faction].add(target)

        # 4. 更新信息熵
        self.faction_maps[faction].update_entropy()

        # 5. 更新障碍图
        if faction == 'white' and self.step % 120 == 0:
            self.faction_maps[faction].get_obstacle_map()

        if faction == 'white':
            # 更新黑方目标探测时间
            for target in self.faction_agents.get('black', []):
                if target.id in self.detection_time_ratios:
                    self.detection_time_ratios[target.id][0] += 1  # 增加总存在时间
                    if target in self.faction_detected['white']:
                        self.detection_time_ratios[target.id][1] += 1  # 增加被探测时间

            # 计算当前步的平均熵值
            total_entropy = 0.0
            valid_targets = 0

            for target_map in self.faction_maps['white'].target_maps.values():
                if target_map.belief > 0.01:  # 仅考虑存在概率>1%的目标
                    total_entropy += target_map.get_entropy()
                    valid_targets += 1

            if valid_targets > 0:
                step_entropy = total_entropy / valid_targets
                self.entropy_history.append(step_entropy)

                # 计算低熵状态（熵值<0.3视为态势把握良好）
                if step_entropy < 0.3:
                    self.low_entropy_count += 1

        # 在态势图更新后添加预测误差统计，仿真开始1h后开始记录
        if faction == 'white' and self.time > 3600:
            total_error = 0.0
            valid_targets = 0

            for target in self.faction_agents.get('black', []):
                if target.id in self.faction_maps['white'].target_maps:
                    # 获取预测位置
                    pred_map = self.faction_maps['white'].target_maps[target.id]
                    pred_x, pred_y = pred_map.position

                    # 计算实际与预测位置的欧氏距离
                    error = math.sqrt((target.x - pred_x) ** 2 + (target.y - pred_y) ** 2)
                    total_error += error
                    valid_targets += 1

            # 计算本步平均误差
            if valid_targets > 0:
                avg_error = total_error / valid_targets
                self.prediction_errors.append(avg_error)
                if self.step % 100 == 0:
                    print(f'{self.step}步态势图平均预测误差: {avg_error:.1f}m')

    def high_level_decision(self, faction: str):

        if faction == 'white':
            """仿真开始时为无人机规划一次广域侦察任务"""
            if self.step == 2:
                # 探索规划
                drone_list = []
                drone_pos_list = []

                if faction == 'white':
                    for agent in self.faction_agents[faction]:
                        if isinstance(agent, Drone):
                            # 对于无人机，进行探索规划
                            drone_list.append(agent)
                            drone_pos_list.append((agent.x, agent.y))

                    # map_poly = [(0, 0), (self.width, 0), (self.width, self.height), (0, self.height)]
                    explore_plan_data, _ = plan_explore_area(
                        boundary=[
                            (0, 0), (self.width, 0), (self.width, self.height), (0, self.height)
                        ],
                        recon_line=[[self.width - self.defense_line_x, 0], [self.width - self.defense_line_x, self.height]],
                        agent_pos_list=drone_pos_list,
                        agent_speed=AGENT_PARAMS['UAV_MAX_VEL'],
                        recon_radius=AGENT_PARAMS['UAV_SCAN_RANGE'],
                        recon_angle=AGENT_PARAMS['UAV_SCAN_ANGLE'] * 2,
                        target_speed=AGENT_PARAMS['BSV_MAX_VEL'],
                        dt=1,
                        require_time=4800,
                        time_now=0,
                    )
                    for agent, plan_data in zip(drone_list, explore_plan_data):
                        agent.set_waypoints([{
                            "position": [pt['position'][0], pt['position'][1], 0.0],  # 初始位置
                            # "arrival_time": pt['arrival_time']  # 初始到达时间
                            "arrival_time": 0  # 全速侦察
                        } for pt in plan_data])
                        agent.task_type = TaskType.EXPLORE
                        agent.take_off()

                    for agent in self.faction_agents[faction]:
                        if isinstance(agent, Drone) and not agent.controller.waypoints:
                            agent.if_explore_done = True
                            agent.task_type = TaskType.IDLE
                    print("explore plan ok")

            """常规高层决策逻辑"""
            # 如果黑方舰船探测到60%，则指挥我方USV进入指定阵位
            if self.boat_occupy_position:
                count_detected_bsv = len(self.faction_detected['black'])
                if len(self.faction_bsv_detected[faction]) > 0.6 * len(self.faction_agents['black']):
                    boat_list = []
                    boat_pos_list = []
                    for agent in self.faction_agents[faction]:
                        if isinstance(agent, Boat):
                            boat_list.append(agent)
                            boat_pos_list.append((agent.x, agent.y))
                    self.set_boat_occupy_position(boat_list, boat_pos_list)
                    self.boat_occupy_position = False

            # 检查飞机是否完成初始阶段的区域探索路线
            if self.step > 5:
                for uav in self.faction_agents[faction]:
                    if isinstance(uav, Drone) and not uav.if_explore_done:
                        if not uav.controller.waypoints:
                            uav.if_explore_done = True
                            if uav.task_type == TaskType.EXPLORE:
                                uav.task_type = TaskType.IDLE

            if self.step % 60 == 0:
                # 调试信息
                if self.debug_mode:
                    print(f"步数 {self.step}: 开始全局协调 (阵营: {faction})")
                    self.cbba_call_count += 1
                    self.last_cbba_call_step = self.step

                try:
                    # 初步侦查终止判定
                    all_explore_done = True
                    # 所有目标被发现
                    if len(self.faction_bsv_detected[faction]) == len(self.faction_maps[faction].target_maps):
                        for agent in self.faction_agents[faction]:
                            if isinstance(agent, Drone) and not agent.if_explore_done:
                                agent.if_explore_done = True
                                agent.task_type = TaskType.IDLE
                                agent.task_list = []
                                agent.set_waypoints([])
                    # 或所有无人机走完预定侦察路线
                    for agent in self.faction_agents[faction]:
                        if isinstance(agent, Drone) and not agent.if_explore_done:
                            all_explore_done = False

                    if all_explore_done:
                        if not self.global_coordinator.all_explore_done:
                            # 完成广域侦察后立刻进行一次无人机停靠目标与无人船打击目标的分配
                            print(f"初始阶段广域侦察任务完成，开始打击任务分配")
                            self.global_coordinator.all_explore_done = True
                            self.global_coordinator.last_uav_charge_allocation_time = -114514
                            self.global_coordinator.last_usv_lock_allocation_time = -114514
                        start_time = time.time()
                        self.global_coordinator.coordination_loop()
                        end_time = time.time()

                        if self.debug_mode:
                            print(f"步数 {self.step}: 全局协调完成，耗时 {end_time - start_time:.3f}秒")

                except Exception as e:
                    print(f"步数 {self.step}: 全局协调失败 - {e}")
                    import traceback
                    traceback.print_exc()
                    # 继续运行，不让一次失败导致整个仿真停止

    def set_boat_occupy_position(self, boat_list, boat_pos_list):
        """
        根据探测到的敌方BSV分布，为我方USV设置待命阵位。
        1. 将战场沿Y轴分为3个区域。
        2. 统计每个区域的敌方BSV数量。
        3. 根据敌方数量按比例分配我方USV到每个区域。
        4. 为每个区域的USV计算均匀分布的待命点。
        5. 向USV下达移动指令。
        """

        if not boat_list:
            print("set_boat_occupy_position: 没有可用的USV")
            return

        # 定义防御区域以及探测到的黑方BSV
        zone_height = self.height / self.num_zones
        detected_usv = self.faction_bsv_detected['white']

        bsv_in_zones = [[] for _ in range(self.num_zones)]

        for bsv in detected_usv:
            bsv_target_map = self.faction_maps['white'].target_maps[bsv.id]
            bsv_y = bsv_target_map.position[1]
            zone_index = int(bsv_y // zone_height)  # TODO 估计位置
            zone_index = max(0, min(zone_index, self.num_zones - 1))
            bsv_in_zones[zone_index].append(bsv)

        # 按比例分配我方USV
        num_our_usv = len(boat_list)
        total_detected_bsv = len(detected_usv)
        usv_allocation = [0] * self.num_zones

        if total_detected_bsv > 0:
            # 按照探测到的BSV数量比例分配
            alloc_float = [(num_our_usv * len(bsv_in_zones[i]) / total_detected_bsv) for i in range(self.num_zones)]
            usv_allocation = [int(n) for n in alloc_float]
            remainder = num_our_usv - sum(usv_allocation)

            if remainder > 0:
                fractional_parts = [alloc_float[i] - usv_allocation[i] for i in range(self.num_zones)]
                indices_to_increment = sorted(range(self.num_zones), key=lambda i: fractional_parts[i], reverse=True)

                for i in range(remainder):
                    usv_allocation[indices_to_increment[i]] += 1
        else:
            # 未探测到BSV，则均匀分配
            base_alloc = num_our_usv // self.num_zones
            remainder = num_our_usv % self.num_zones
            for i in range(self.num_zones):
                usv_allocation[i] = base_alloc + (1 if i < remainder else 0)

        # 计算阵位并派遣USV
        stand_by_positions = []
        for i in range(self.num_zones):
            num_usvs_in_zone = usv_allocation[i]
            if num_usvs_in_zone == 0:
                continue

            zone_start_y = i * zone_height
            # 在区域内均匀计算Y坐标
            # TODO： 后续改进优化最小距离？
            for j in range(num_usvs_in_zone):
                standby_y = zone_start_y + (j + 1) * (zone_height / (num_usvs_in_zone + 1))
                stand_by_positions.append([self.defense_line_x, standby_y])

        avaliable_boats = list(boat_list)  # 创建可变副本
        for pos_x, pos_y in stand_by_positions:
            if not avaliable_boats:
                break

            # 寻找距离当前阵位最近的USV
            closest_boat = min(avaliable_boats, key=lambda usv: math.sqrt((usv.x - pos_x) ** 2 + (usv.y - pos_y) ** 2))

            # 分配任务并从可用列表中移除
            if closest_boat:
                waypoint = [{"position": [pos_x, pos_y, 0.0], "arrival_time": 0.0}]
                closest_boat.set_waypoints(waypoint)
                avaliable_boats.remove(closest_boat)

    def check_collisions(self):
        """简单的碰撞检测"""
        for i, agent1 in enumerate(self.agents):
            for agent2 in self.agents[i + 1:]:
                if isinstance(agent1, Drone) and isinstance(agent2, Drone): continue  # 跳过无人机之间的碰撞检测
                distance = math.sqrt((agent1.x - agent2.x) ** 2 + (agent1.y - agent2.y) ** 2)
                if distance < (agent1.radius + agent2.radius):
                    agent1.on_collision(agent2)
                    agent2.on_collision(agent1)

    def check_winning(self):
        for agent in self.agents[:]:
            if agent.faction == 'black' and agent.x < self.defense_line_x:
                self.break_through.append(agent)  # 录入突防成功单位列表
                if self.time < self.break_through_time: self.break_through_time = self.time
                self.faction_maps['white'].remove_target(agent.id)  # 从白方态势图中移除已突防的单位
                self.remove_agent(agent)  # 从世界中移除突防单位
        remaining_factions = []
        for faction in self.faction_agents:
            if len(self.faction_agents[faction]) > 0:
                remaining_factions.append(faction)
        if len(remaining_factions) <= 1:
            self.running = False
            if len(remaining_factions) == 1:
                self.winner = remaining_factions[0]

    def get_agents_in_range(self, x: float, y: float, range_: float) -> List['Agent']:
        """获取指定范围内的所有智能体"""
        return [agent for agent in self.agents
                if math.sqrt((agent.x - x) ** 2 + (agent.y - y) ** 2) <= range_]

    def get_agents_near(self, center: Agent, range_: float) -> List['Agent']:
        """获取指定智能体周围一定范围内的所有智能体"""
        return self.get_agents_in_range(center.x, center.y, range_)

    def save_current_figure(self):
        """保存当前图窗为图片"""
        # 确保log目录存在
        log_dir = "log"
        if not os.path.exists(log_dir):
            os.makedirs(log_dir)

        # 生成带时间戳和步数的文件名
        timestamp = time.strftime("%Y%m%d_%H%M%S")
        filename = f"case_{self.case}_step_{self.step}_{timestamp}.png"
        filepath = os.path.join(log_dir, filename)

        # 保存当前图窗
        if self.visualize and self.visualizer:
            plt.figure(self.visualizer.fig.number)  # 确保保存的是当前图窗
            plt.savefig(filepath, dpi=300)
            print(f"当前图窗已保存至: {filepath}")

    def on_key_press(self, event):
        """处理键盘事件"""
        if event.key == '+':
            # 增加仿真速度
            self.speed_factor = min(100, self.speed_factor + 5)
        elif event.key == '-':
            # 降低仿真速度
            self.speed_factor = max(1, self.speed_factor - 5)
        elif event.key == ' ':
            # 暂停/继续仿真
            self.paused = not self.paused
        elif event.key == 'r':
            # 切换范围显示
            self.show_ranges = not self.show_ranges
        elif event.key == 't':
            # 切换文本标签显示
            self.show_text = not self.show_text
        elif event.key == 'c':  # 添加保存图窗的按键
            # 保存当前图窗
            self.save_current_figure()
        elif event.key == 'q':
            # 退出仿真
            self.running = False
            if self.visualize and self.visualizer:
                self.visualizer.close()

        # 更新状态显示
        if self.visualize and self.visualizer:
            self.visualizer.update_status()

    def run_simulation(self, max_step: int = 10000000):
        """运行仿真"""
        self.running = True
        self.init_situation_maps(self.grid_resolution)
        self.init_high_level_decision()

        # 调试：记录开始时间
        simulation_start_time = time.time()
        last_update_time = simulation_start_time
        step_timeout = 10.0  # 单步最大执行时间（秒）

        # 推进仿真运行
        while self.running and self.step <= max_step:
            step_start_time = time.time()

            # 调试：监控步数进度
            if self.debug_mode and self.step % 60 == 0:
                elapsed = step_start_time - simulation_start_time
                print(f"仿真步数: {self.step}, 总耗时: {elapsed:.1f}秒, CBBA调用次数: {self.cbba_call_count}")

            # 更新仿真状态
            try:
                self.update()
                step_end_time = time.time()
                step_duration = step_end_time - step_start_time

                # 检查单步是否超时
                if step_duration > step_timeout:
                    print(f"警告：步数 {self.step} 执行超时 ({step_duration:.2f}秒)")
                    if self.debug_mode:
                        print(f"距离上次CBBA调用: {self.step - self.last_cbba_call_step} 步")

            except KeyboardInterrupt:
                print("仿真被用户中断")
                self.running = False
                break
            except Exception as e:
                print(f"仿真步数 {self.step} 发生错误: {e}")
                import traceback
                traceback.print_exc()
                self.running = False
                break

            # 更新可视化
            render_interval = self.step - self.last_draw_step
            if self.visualize and render_interval % self.speed_factor == 0:
                self.visualizer.update_visualization()
                self.last_draw_step = self.step

                # 处理暂停状态
                while self.paused and self.running:
                    self.visualizer.pause_handler()
                if not self.visualizer.is_active():
                    self.running = False
                    break

        self.save_log_file()  # 保存日志文件

        # 打印胜利者
        print("-" * 40)
        if self.winner is not None:
            print(f"Winner: {self.winner}")
            print(f"Black break through count: {len(self.break_through)}")
            print(f"White collision count: {self.white_collision_count}")
            print(f"Freeze count: Black {self.black_freeze_count} : {self.white_freeze_count} White")
            print(f"Intercept time: {self.break_through_time/3600} h")
        else:
            print("Draw!")
            print(f"Black break through count: {len(self.break_through)}")
            print(f"White collision count: {self.white_collision_count}")
            print(f"Freeze count: Black {self.black_freeze_count} : {self.white_freeze_count} White")
            print(f"Intercept time: {self.break_through_time/3600} h")

        # 清理HTN资源
        self.cleanup_htn_resources()

        # 仿真结束后绘制性能指标图
        self.plot_performance_metrics()

        # 仿真结束后保持窗口开启
        if self.visualize and self.visualizer:
            self.visualizer.show()

    def save_log_file(self):
        """保存日志文件到log目录"""

        # 计算平均位置预测误差
        if self.prediction_errors:
            avg_prediction_error = sum(self.prediction_errors) / len(self.prediction_errors)
        else:
            avg_prediction_error = 0

        # 计算平均探测时间占比
        detection_ratios = []
        for target_id, (total_time, detected_time) in self.detection_time_ratios.items():
            if total_time > 0:
                ratio = detected_time / total_time
                detection_ratios.append(ratio)
        if detection_ratios:
            self.avg_detection_ratio = sum(detection_ratios) / len(detection_ratios)
            self.detection_ratios_per_target = detection_ratios
        else:
            self.avg_detection_ratio = 0.0
            self.detection_ratios_per_target = []

        # 计算防御效率
        interception_rate = 0
        if self.faction_bsv_detected.get('white'):
            interception_rate = 1 - (len(self.break_through) / len(self.faction_bsv_detected['white']))

        # 计算全程平均熵值和低熵状态占比
        if self.entropy_history:
            self.avg_entropy = sum(self.entropy_history) / len(self.entropy_history)
            self.low_entropy_ratio = self.low_entropy_count / len(self.entropy_history)

        log = {
            "case": self.case,
            "black_break_through": len(self.break_through),
            "white_collision": self.white_collision_count,
            "black_freeze": self.black_freeze_count,
            "white_freeze": self.white_freeze_count,
            "intercept_time": self.break_through_time,
            'interception_rate': interception_rate,
            'avg_prediction_error': avg_prediction_error,
            "detection_time_ratio": self.avg_detection_ratio,
            "avg_entropy": self.avg_entropy,
            "low_entropy_ratio": self.low_entropy_ratio,
            "logger": self.log_data
        }

        # 确保log目录存在
        log_dir = "log"
        if not os.path.exists(log_dir):
            os.makedirs(log_dir)

        # 生成带时间戳的文件名
        timestamp = time.strftime("%Y%m%d_%H%M%S")
        filename = f"case_{self.case}_log_{timestamp}.json"
        filepath = os.path.join(log_dir, filename)

        # 保存JSON文件
        with open(filepath, 'w', encoding='utf-8') as f:
            json.dump(log, f, ensure_ascii=False, indent=4)

        print(f"仿真日志已保存至: {filepath}")

        # 打印新增指标
        print(f"全程平均预测误差: {avg_prediction_error:.2f} 米")
        print(f"平均探测时间占比: {self.avg_detection_ratio * 100:.2f}%")  # 打印探测时间占比
        print(f"平均归一化战场信息熵: {self.avg_entropy:.2f}")
        print(f"态势高确定性时间占比: {self.low_entropy_ratio * 100:.2f}%")
        print(f"拦截率: {interception_rate * 100:.2f}%")

    def plot_performance_metrics(self):
        """绘制性能指标变化图"""
        plt.figure(figsize=(10, 5))

        # 1. 熵值变化曲线
        plt.subplot(1, 2, 1)
        if self.entropy_history:
            steps = np.arange(len(self.entropy_history)) * self.log_interval
            plt.plot(steps, self.entropy_history, 'b-', linewidth=1.5)
            plt.axhline(y=0.3, color='r', linestyle='--', alpha=0.7)
            plt.fill_between(steps, 0, self.entropy_history, where=(np.array(self.entropy_history) < 0.3),
                             color='green', alpha=0.3, interpolate=True)
            plt.xlabel('仿真步数')
            plt.ylabel('平均熵值')
            plt.title('战场信息熵随时间变化')
            plt.grid(True)
            plt.legend(['平均熵值', '低熵阈值(0.3)', '低熵区域'])

            # 添加统计信息
            stats_text = (f'全程平均熵值: {self.avg_entropy:.4f}\n'
                          f'低熵状态占比: {self.low_entropy_ratio * 100:.2f}%')
            plt.annotate(stats_text, xy=(0.95, 0.95), xycoords='axes fraction',
                         ha='right', va='top', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))

        # 2. 预测误差变化曲线
        plt.subplot(1, 2, 2)
        if self.prediction_errors:
            # 计算误差数据的起始步数（从3600秒后开始）
            start_step = int(3600 / self.interval)
            steps = np.arange(len(self.prediction_errors)) * self.log_interval + start_step

            plt.plot(steps, self.prediction_errors, 'r-', linewidth=1.5)
            plt.xlabel('仿真步数')
            plt.ylabel('平均预测误差(米)')
            plt.title('目标位置预测误差随时间变化')
            plt.grid(True)

            # 添加统计信息
            avg_error = sum(self.prediction_errors) / len(self.prediction_errors)
            min_error = min(self.prediction_errors) if self.prediction_errors else 0
            max_error = max(self.prediction_errors) if self.prediction_errors else 0

            stats_text = (f'全程平均误差: {avg_error:.2f}米\n'
                          f'最小误差: {min_error:.2f}米\n'
                          f'最大误差: {max_error:.2f}米')
            plt.annotate(stats_text, xy=(0.95, 0.95), xycoords='axes fraction',
                         ha='right', va='top', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))

        # 添加主标题
        plt.suptitle(f'仿真性能评估指标 (案例: {self.case}, 总步数: {self.step})', fontsize=16)

        # 保存图像
        log_dir = "log"
        if not os.path.exists(log_dir):
            os.makedirs(log_dir)
        timestamp = time.strftime("%Y%m%d_%H%M%S")
        filename = f"case_{self.case}_metrics_{timestamp}.png"
        filepath = os.path.join(log_dir, filename)
        plt.savefig(filepath, dpi=300)
        print(f"性能指标图已保存至: {filepath}")

        # 显示图像
        plt.show()

    def cleanup_htn_resources(self):
        """清理HTN相关资源"""
        try:
            if hasattr(self, 'global_coordinator'):
                self.global_coordinator.stop_coordination()
                print("HTN全局协调器已关闭")
        except Exception as e:
            print(f"HTN资源清理失败: {e}")

    def get_all_white_boats(self) -> List[Boat]:
        """获取所有未冻结的白方USV"""
        return [agent for agent in self.faction_agents['white'] if
                isinstance(agent, Boat) and not agent.freeze and agent.world == self]

    def load_black_agents_from_scheme(self, scheme_file: str):
        """
        从方案文件加载黑方单位并设置路径点

        参数:
            scheme_file: 方案文件的路径
        """
        try:
            with open(scheme_file, 'r', encoding='utf-8') as f:
                scheme_data = json.load(f)

            usv_num = scheme_data.get("usvNum", 0)
            print(f"从方案文件 {scheme_file} 加载 {usv_num} 个黑方单位")

            for i in range(1, usv_num + 1):
                key = f"usv{i}"
                if key not in scheme_data:
                    continue

                path = scheme_data[key]
                if not path:
                    continue

                # 使用第一个路径点作为初始位置
                start_point = path[0]["point"]
                x = start_point[0]
                y = start_point[1]

                # 计算初始航向（如果有多于一个点）
                heading = math.pi  # 默认航向（朝向左侧）
                if len(path) > 1:
                    next_point = path[1]["point"]
                    dx = next_point[0] - x
                    dy = next_point[1] - y
                    heading = math.atan2(dy, dx)

                # 创建黑方无人艇
                boat = Boat(
                    x=x, y=y, heading=heading,
                    faction="black",
                    sensor_range=AGENT_PARAMS['BSV_SCAN_RANGE'],
                    lock_range=AGENT_PARAMS['BSV_LOCK_RANGE']
                )

                # 准备航路点
                waypoints = []
                current_time = self.time

                for j, point in enumerate(path):
                    # 跳过第一个点（已经是初始位置）
                    if j == 0:
                        continue

                    position = [point["point"][0], point["point"][1], 0]

                    # 计算到达时间（基于速度和距离）
                    prev_point = path[j - 1]["point"]
                    dx = position[0] - prev_point[0]
                    dy = position[1] - prev_point[1]
                    distance = math.sqrt(dx ** 2 + dy ** 2)
                    speed = point.get("speed", 10) / 10 * AGENT_PARAMS['BSV_MAX_VEL']  # 默认速度10 m/s

                    # 计算时间间隔
                    time_interval = distance / speed if speed > 0 else 0
                    current_time += time_interval

                    waypoints.append({
                        "position": position,
                        "arrival_time": current_time
                    })

                # 设置航路点
                if waypoints:
                    boat.set_waypoints(waypoints)

                # 添加到世界
                self.add_agent(boat)
                print(f"添加黑方单位 {key} 在位置 ({x}, {y})，设置 {len(waypoints)} 个航路点")

        except Exception as e:
            print(f"加载方案文件 {scheme_file} 失败: {e}")
            import traceback
            traceback.print_exc()


def main():
    """主函数，运行仿真"""
    from agent import AGENT_PARAMS

    # 创建世界
    world = World(width=300000, height=673205, interval=1.0, visualize=True, debug_mode=True, case=7)
    # 添加一些舰船
    # 红方舰船自左边底边中点间隔5km展开
    # 黑方舰船按选定的初始化方案自动生成
    sign = 1
    for i in range(5):
        world.add_agent(Boat(x=0, y=world.height / 2 + i * sign * 5000, heading=0,
                             faction="white",
                             sensor_range=AGENT_PARAMS['USV_SCAN_RANGE'],
                             lock_range=AGENT_PARAMS['USV_LOCK_RANGE'],
                             has_drone=True))
        sign *= -1
    # 运行仿真
    world.run_simulation()

if __name__ == "__main__":
    main()
